#include <stdint.h>
#include <cmath>

uint32_t control_PPS(double *last_val, uint8_t *flag, double clock_offset_ns){
    // last_val已调节量，单位为ns
    // flag每次发送指令，都需要翻转，主要用来判断指令是否重复发送
    // clock_offset_ns 两节点钟差，单位为 ns
    int RX_SAMPLE_RATE = 4e7;
    const double sample_ns = 1.0e9/RX_SAMPLE_RATE;
    // 1 ns需要的步进次数
    const double steps_ns = 56;
    // 控制字
    uint32_t control_word = 0;

    // 待调节量
    double offset_need = clock_offset_ns - (*last_val);
    int integer_cycle = 0;
    int VCO_phase = 0;
    // [31:13]          [12]        [11:0]
    // 整数ADC时钟个数   flag       小于一个时钟ADC的数
    // 整数时钟可能一次无法调整到位，需要多次调整，针对20M采样率，20_000_000，需要25bit才够，这里仅有19bit，
    // 但通常只需要在初始阶段需要大的调整
    // 对于小数部分，每次步进量为 1/56 ns，如果要调整1个ADC的量，需要调整 50ns / (1/56) = 2800次，需要12bit表示
    integer_cycle = floor(offset_need/sample_ns);
    if(integer_cycle!=0){
        // 需要调大于一个采样点
        // 判断正向调整还是负向调整
        if(integer_cycle>0){
            if(integer_cycle>262143){
                // 最大调整量 2^18-1，
                integer_cycle = 262143;
            }
            control_word = (integer_cycle<<13);
            // 计算剩余待调整量
        }
        else{
            if(integer_cycle<-262144){
                // 最大调整量 -2^18，
                integer_cycle = -262144;
            }
            control_word = (integer_cycle<<13);
        }
    }
    // 剩余待调节量
    double frac = offset_need - integer_cycle*sample_ns;
    if(frac>0 && frac<sample_ns){
        // 如果大于1个采样周期，本次不调整小数部分，小于1个采样周期，进行调整
        VCO_phase =  round(frac*steps_ns);
        control_word += VCO_phase;
    }

    // 写入flag并翻转
    control_word += ((*flag)<<12);
    *flag = !(*flag);
    // 计算本次已经调节量
    double offset_finish = integer_cycle*sample_ns + VCO_phase*1.0/steps_ns;
    *last_val = *last_val + offset_finish;
    return control_word;
}